use std::cmp::{max, min};
use std::collections::linked_list::LinkedList;
use std::time::Duration;
use bevy::prelude::*;

use crate::{CurrentFloor, ELEVATOR_TOP_FLOOR, OpenTimer, ReqQueue};
use crate::components::Elevator;

pub const PIXEL_PER_FLOOR: i32 = 60;
const MAX_SPEED: f32 = 60.;
const MIN_SPEED: f32 = 30.;
const MAX_ACC: f32 = 30.;  // 加速到最大速度需要2s，耗费的距离是 60px
const ELEVATOR_BOTTOM_POSITION: f32 = -220.0;
const INVALID_FLOOR: i32 = 0;
const OPENING_TIME: u64 = 2;

pub struct MovementPlugin;

#[derive(Default, Debug)]
pub struct ButtonState {
    pub floor_button: [bool; ELEVATOR_TOP_FLOOR as usize],
    pub up_button: [bool; ELEVATOR_TOP_FLOOR as usize],
    pub down_button: [bool; ELEVATOR_TOP_FLOOR as usize],
}

impl Plugin for MovementPlugin {
    fn build(&self, app: &mut App) {
        app.add_startup_system(spawn_elevator)
            .add_system(movement_planning_system)
            .add_system(elevator_accelerate_system)
            .add_system(elevator_movement_system)
            .add_system(update_current_floor)
            .add_system(open_timer_system)
            .insert_resource(ButtonState::default());
    }
}

fn spawn_elevator(mut commands: Commands) {
    commands
        .spawn_bundle(SpriteBundle {
            transform: Transform {
                translation: Vec3::new(0.0, ELEVATOR_BOTTOM_POSITION, 0.0),
                scale: Vec3::new(40.0, 40.0, 0.0),
                ..Default::default()
            },
            sprite: Sprite {
                color: Color::rgb(0.5, 0.5, 1.0),
                ..Default::default()
            },
            ..Default::default()
        })
        .insert(Elevator {speed: 0., accelerate: 0., stop_at: INVALID_FLOOR})
        .insert(ReqQueue { request_queue : LinkedList::new() });
}

fn elevator_movement_system(
    mut elevator: Query<(&mut Transform, &mut Elevator), Without<OpenTimer>>,
    time: Res<Time>,
) {
    for (mut transform, mut elevator) in elevator.iter_mut() {
        transform.translation.y += elevator.speed * time.delta_seconds();
        let speed: f32 = elevator.speed + elevator.accelerate * time.delta_seconds();
        if speed.abs() > MAX_SPEED {
            if elevator.speed > 0. {
                elevator.speed = MAX_SPEED;
            } else {
                elevator.speed = -MAX_SPEED;
            }
        } else {
            elevator.speed = speed;
        }
    }
    // let (mut transform, mut elevator) = elevator.single_mut(); // 改为iter的形式，防止single_mut不存在
}

// 根据停靠楼层改变电梯的加速度
// 如果stop_at == current_floor, 说明已经到达需要停靠的楼层
fn elevator_accelerate_system(
    mut commands: Commands,
    mut query: Query<(Entity, &mut Elevator, &mut ReqQueue), Without<OpenTimer>>,
    current_floor: Res<CurrentFloor>,
    mut button_state: ResMut<ButtonState>,
) {
    for (entity, mut elevator, mut req) in query.iter_mut() {
        if elevator.stop_at == INVALID_FLOOR {
            if elevator.speed == 0. && elevator.accelerate == 0. {
                // 清空静止状态的同楼层请求
                button_state.up_button[current_floor.0 as usize - 1] = false;
                button_state.down_button[current_floor.0 as usize - 1] = false;
                button_state.floor_button[current_floor.0 as usize - 1] = false;
            }
            return;
        }
        // 请求停靠楼层在当前楼层之上
        if elevator.stop_at > current_floor.0 {
            // 停靠楼层紧邻当前楼层
            if elevator.stop_at - 1 == current_floor.0 {
                if elevator.speed > MIN_SPEED {
                    elevator.accelerate = - MAX_ACC;
                } else if elevator.speed < MIN_SPEED {
                    elevator.accelerate = MAX_ACC;
                } else {
                    elevator.accelerate = 0.;
                }
            } else {
                if elevator.speed < MAX_SPEED {
                    elevator.accelerate = MAX_ACC;
                } else {
                    elevator.accelerate = 0.;
                }
            }
        } else if elevator.stop_at < current_floor.0 {
            // 请求停靠楼层在当前楼层之下
            if elevator.stop_at + 1 == current_floor.0 {
                if elevator.speed < -MIN_SPEED {
                    // 向上减速
                    elevator.accelerate = MAX_ACC;
                } else if elevator.speed > -MIN_SPEED {
                    // 向下加速
                    elevator.accelerate = -MAX_ACC;
                } else {
                    // 匀速抵达
                    elevator.accelerate = 0.;
                }
            }
        } else {
            // 已经到达请求楼层
            elevator.speed = 0.;
            elevator.accelerate = 0.;
            elevator.stop_at = INVALID_FLOOR;
            commands.entity(entity).insert(OpenTimer{
                timer: Timer::new(Duration::from_secs(OPENING_TIME), false),
            });
            button_state.up_button[current_floor.0 as usize - 1] = false;
            button_state.down_button[current_floor.0 as usize - 1] = false;
            button_state.floor_button[current_floor.0 as usize - 1] = false;
            println!("door opening");
            // todo: does this needed in here? Yes! This is the clear signal send by the elevator not by the button
            let n = req.request_queue.len();
            for _i in 0..n {
                let ele = req.request_queue.pop_front().unwrap();
                if ele.at != current_floor.0 {
                    req.request_queue.push_back(ele);
                }
            }
        }
    }
}

fn update_current_floor(
    mut query: Query<&Transform, With<Elevator>>,
    mut current_floor: ResMut<CurrentFloor>,
) {
    let transform = query.single_mut();
    current_floor.0 = y_to_floor(&transform.translation);
}

fn y_to_floor(translation: &Vec3) -> i32 {
    (translation.y as i32 - ELEVATOR_BOTTOM_POSITION as i32) / PIXEL_PER_FLOOR + 1
}

// decide the elevator movement based on the Request Queue
fn movement_planning_system(
    mut elevator: Query<(&mut Elevator, &ReqQueue), Changed<ReqQueue>>,
    button_state: Res<ButtonState>,
    current_floor: Res<CurrentFloor>,
) {
    for (mut elevator, reqQueue) in elevator.iter_mut(){
        // there is request
        if let Some(req) = reqQueue.request_queue.front() {
            let (down_far, down_near, up_near, up_far) = get_four_request(current_floor.0 as usize, &button_state.floor_button, &button_state.down_button, &button_state.up_button);
            // 电梯处于静止状态
            if elevator.speed == 0.0 && elevator.accelerate == 0.0 {
                // 还有未完成的远端请求
                if req.at > current_floor.0 {
                    elevator.stop_at = up_near;
                } else {
                    elevator.stop_at = down_near;
                }
            } else if elevator.speed > 0.0 && elevator.accelerate == 0.0 {
                // 电梯处于匀速向上状态
                if up_near != INVALID_FLOOR && up_near - current_floor.0 > 1 {
                    // 存在一层楼的减速距离
                    elevator.stop_at = up_near;
                } else if up_far != INVALID_FLOOR {
                    // 看看是否存在更远端的请求
                    elevator.stop_at = up_far;
                }
            } else if elevator.speed < 0.0 && elevator.accelerate == 0.0 {
                // 电梯处于匀速向下状态
                if down_near != INVALID_FLOOR && current_floor.0 - down_near > 1 {
                    elevator.stop_at = down_near;
                } else if up_far != INVALID_FLOOR {
                    elevator.stop_at = down_far;
                }
            } else if elevator.speed >= 0.0 && elevator.accelerate > 0.0 {
                // 向上加速
                if up_near != INVALID_FLOOR && up_near < elevator.stop_at {
                    if up_near == current_floor.0 + 1 {
                        // 在相邻的楼层有请求，判断当前的速度是否可以停下
                        if can_stop_at_next(elevator.speed) {
                            elevator.stop_at = up_near;
                        }
                    } else {
                        elevator.stop_at = up_near;
                    }
                }
            } else if elevator.speed >= 0.0 && elevator.accelerate < 0.0 {
                // 向上减速: do nothing
            } else if elevator.speed <= 0.0 && elevator.accelerate < 0.0 {
                // 向下加速
                if down_near != INVALID_FLOOR && down_near > elevator.stop_at {
                    if down_near == current_floor.0 - 1 {
                        if can_stop_at_next(elevator.speed) {
                            elevator.stop_at = down_near;
                        }
                    } else {
                        elevator.stop_at = down_near;
                    }
                }
            } else {
                // 向下减速
                // do nothing
            }
        } else {
            // request queue清空了
            if can_stop_at_next(elevator.speed) {
                if elevator.speed > 0.0 {
                    elevator.stop_at = current_floor.0 + 1; // todo: does this need max/min?
                } else if elevator.speed < 0.0 {
                    elevator.stop_at = current_floor.0 - 1;
                }
            } else {
                // 当前的速度不能在相邻层停止
                if elevator.speed > 0.0 {
                    elevator.stop_at = min(current_floor.0 + 2, ELEVATOR_TOP_FLOOR as i32);
                } else if elevator.speed < 0.0 {
                    elevator.stop_at = max(current_floor.0 - 2, 1);
                }
            }
        }
        println!("ReqQueue changed");
    }
}

fn open_timer_system(
    mut commands: Commands,
    mut q: Query<(Entity, &mut OpenTimer)>,
    time: Res<Time>,
) {
    // tick the timer
    for (entity, mut open_timer) in q.iter_mut() {
        // timers gotta be ticked, to work
        open_timer.timer.tick(time.delta());

        // if it finished, despawn the Timer
        if open_timer.timer.finished() {
            commands.entity(entity).remove::<OpenTimer>();
            println!("door closed");
        }
    }
}

// 判断是否可以以当前位置的速度停在下一层
// 2 * 1/2 * v * t < PIXEL_PER_FLOOR
fn can_stop_at_next(speed : f32) -> bool {
    return if speed * (speed / MAX_ACC) > PIXEL_PER_FLOOR as f32 {
        false
    } else {
        true
    }
}

// 返回相对于当前楼层的最近、次最近的四个请求，返回值为四个楼层号（从1到ELEVATOR_TOP_FLOOR)，不包含当前楼层
fn get_four_request(
    mut current_floor: usize,
    floor_button: &[bool; ELEVATOR_TOP_FLOOR],
    down_button: &[bool; ELEVATOR_TOP_FLOOR],
    up_button: &[bool; ELEVATOR_TOP_FLOOR]
) -> (i32, i32, i32, i32) {
    let mut combined_state: [bool; ELEVATOR_TOP_FLOOR] = *floor_button; // copy assignment
    for (i, x) in combined_state.iter_mut().enumerate() {
        *x |= down_button[i];
        *x |= up_button[i];
    }
    let (mut down_near, mut down_far, mut up_near, mut up_far) = (INVALID_FLOOR, INVALID_FLOOR, INVALID_FLOOR, INVALID_FLOOR);
    current_floor -= 1; // 注意传递是楼层号
    for (i, x) in combined_state.iter().enumerate() {
        if *x {
            if i < current_floor {
                if down_far == INVALID_FLOOR {
                    down_far = i as i32 + 1;
                } else if down_near == INVALID_FLOOR {
                    down_near = i as i32 + 1;
                } else {
                    down_far = down_near;
                    down_near = i as i32 + 1;
                }
            } else if i > current_floor {
                if up_near == INVALID_FLOOR {
                    up_near = i as i32 + 1;
                } else if up_far == INVALID_FLOOR {
                    up_far = i as i32 + 1;
                }
            }
        }
    }
    (down_far, down_near, up_near, up_far)
}

#[test]
fn test_array() {
    let mut arr = [false; ELEVATOR_TOP_FLOOR];
    arr[1] = true;
    arr[2] = true;
    arr[3] = true;
    arr[5] = true;
    // arr[6] = true;
    // arr[7] = true;
    let (down_far, down_near, up_near, up_far) = get_four_request(4, &arr, &arr, &arr);
    assert_eq!(down_far, 2);
    assert_eq!(down_near, 3);
    assert_eq!(up_near, 6);
    assert_eq!(up_far, INVALID_FLOOR);
}

fn foo(arr: &[bool; 5]) {
    let mut arr2 = *arr;
    arr2[3] = true;
}